#include <Arduino.h>
#include<Ticker.h>
#include <Esp32McpwmMotor.h>
#include <Wire.h> // 包含I2C库
#include <Adafruit_GFX.h> // 包含Adafruit的图形库
#include <Adafruit_SSD1306.h> // 包含Adafruit的SSD1306 OLED驱动库
#include "PidController.h"

#define SCREEN_WIDTH 128 // OLED屏幕的宽度，单位为像素
#define SCREEN_HEIGHT 64 // OLED屏幕的高度，单位为像素

// 初始化一个与SSD1306兼容的OLED显示对象
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);

Ticker timer_read_encoder;  // 中断函数定时器定义

Esp32McpwmMotor espMotor; // 创建一个名为espMotor的对象，用于控制电机
#define enA1 4
#define enB1 5
#define enA2 18
#define enB2 19
#define IN1 33
#define IN2 26
#define ENA 25
#define IN3 27
#define IN4 12
#define ENB 14

float Kp=0.05, Ki=0.35, Kd=0.5;
//设定值Setpoint，测速值Input，pid之后的输出值Output
float Setpoint1, Input1, Output1;
float Setpoint2, Input2, Output2;

PidController pid_controller[2];   // 创建PidController的两个对象

int16_t sENA=0, sENB=0;//占空比-100~100
int interrupt_time=50;

void motorOUT(void);
void readVel(void);
void READ_ENCODER_1(void);
void READ_ENCODER_2(void);

int32_t enL,enR;
float motorLOOP=11.0*19.0;//线数x减速比
float velL,velR;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

  // 初始化OLED显示
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // 初始化并检查OLED屏幕与I2C地址0x3C是否连接
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // 如果失败，停止
  }

  espMotor.attachMotor(0,IN2,IN1);
  espMotor.attachMotor(1,IN3,IN4);
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  pinMode(enA1,INPUT);
  pinMode(enB1,INPUT);
  pinMode(enA2,INPUT);
  pinMode(enB2,INPUT);
  pid_controller[0].update_pid(Kp, Ki, Kd);
  pid_controller[1].update_pid(Kp, Ki, Kd);
  Setpoint1=80.0;
  Setpoint2=80.0;
  pid_controller[0].update_target(Setpoint1); // 更新控制器的目标值
  pid_controller[1].update_target(Setpoint2);
  //编码器接口1 开启外部跳边沿中断 
  attachInterrupt(enA1,READ_ENCODER_1,CHANGE);//中断函数READ_ENCODER_1
  //编码器接口2 开启外部跳边沿中断 
  attachInterrupt(enA2,READ_ENCODER_2,CHANGE);//中断函数READ_ENCODER_2
  interrupts();//打开外部中断
  // 初始化PID控制器的最大输入输出，MPCNT大小范围在正负100之间
  pid_controller[0].out_limit(-400.0, 400.0);
  pid_controller[1].out_limit(-400.0, 400.0);
  //
  timer_read_encoder.attach_ms(interrupt_time,readVel);
}

void loop() {
  // put your main code here, to run repeatedly:
  /*
  if (Serial.available()>0)
  {
    char receivedChar = Serial.read();
    if (receivedChar == 't')
    {
      for (int16_t i = 0; i < 101; i++)
      {
        sENA=i;
        sENB=i;
        motorOUT();
        delay(10000);
        Serial.print("pwm1=");
        Serial.print(sENA);
        Serial.print("; pwm2=");
        Serial.print(sENB);
        Serial.print("; speed1=");
        Serial.print(velL);
        Serial.print("; speed2=");
        Serial.print(velR);
        Serial.print("; count1=");
        Serial.print(enL);
        Serial.print("; count2=");
        Serial.println(enR);
      }
    } 
  }
  */
  // 清除屏幕缓冲区
  display.clearDisplay();
  // 设置文本属性
  display.setTextSize(1);      // 设置文本大小
  display.setTextColor(SSD1306_WHITE); // 设置文本颜色为白色
  display.setCursor(0,0);     // 设置文本光标位置
  // 显示文本
  display.print("left speed=");
  display.println(Input1);
  display.print("right speed=");
  display.println(Input2);
  // 显示缓冲区到屏幕上
  display.display();
  delay(50);
}

void motorOUT(){
  //转换转速到pwm
  float OutPwm1,OutPwm2;
  if (Output1>=0.0)
  {
    OutPwm1=(Output1-23.69)/5.59;
  }
  else
  {
    OutPwm1=(Output1+23.69)/5.59;
  }
   if (Output2>=0.0)
  {
    OutPwm2=(Output2-14.0)/5.69;
  }
  else
  {
    OutPwm2=(Output2+14.0)/5.69;
  }
  sENA=(int16_t)OutPwm1;
  sENB=(int16_t)OutPwm2;
  // sENA=(int16_t)Output1;
  // sENB=(int16_t)Output2;
  // sENA=50;
  // sENB=50;
  espMotor.updateMotorSpeed(0,sENA);
  espMotor.updateMotorSpeed(1,sENB);
}

void readVel(){
  float enLf=(float)enL;
  float enRf=(float)enR;
  enL=0;
  enR=0;
  float in_time=((float)interrupt_time)/1000.0;
  velL=(enLf/motorLOOP)/in_time*60.0;
  velR=(enRf/motorLOOP)/in_time*60.0;
  Input1=(double)velL;
  Input2=(double)velR;
  // 计算最新的电机输出值
  Output1 = pid_controller[0].update(Input1);
  Output2 = pid_controller[1].update(Input2);
  motorOUT();
}

void READ_ENCODER_1(void){
  if (digitalRead(enA1)==LOW)
  {
    if (digitalRead(enB1)==LOW)
    {
      enL--;
    }
    else
    {
      enL++;
    }
  }
}

void READ_ENCODER_2(void){
  if (digitalRead(enA2)==LOW)
  {
    if (digitalRead(enB2)==LOW)
    {
      enR++;
    }
    else
    {
      enR--;
    }
  }
}